#include <ros/ros.h>
#include <rei_robot_base/Int8.h>
#include <rei_robot_base/CarData.h>

bool LIGHT_IO = 0;
ros::ServiceClient motorCtrlClient;
void LightSensor_cb(const rei_robot_base::CarData & car_data){
    LIGHT_IO = car_data.input_io[0];//获取传感器状态
    if (LIGHT_IO == 1){
        rei_robot_base::Int8 stop;
        stop.request.data = 0;
        motorCtrlClient.call(stop);
    }
}

bool ConveyerCallback(rei_robot_base::Int8::Request &req,
                      rei_robot_base::Int8::Response &res){
    rei_robot_base::Int8 srv;
    ros::Rate loop(10);
    if (LIGHT_IO == 0){
        if (req.data == 1){
            srv.request.data = 1;
            res.success = true;
            res.message = "motor is running forward";
        }else if(req.data == 2){
            srv.request.data = -1;
            res.success = true;
            res.message = "motor is reversing";
        }else{
            srv.request.data = 0;
            res.success = true;
            res.message = "motor stop";
        }
    }else{
        srv.request.data = 0; 
        res.success = false;
        res.message = "lightSensor active";
    }
    motorCtrlClient.call(srv);
    return true;
}

int main(int argc,char** argv){
    ros::init(argc,argv,"conveyer_control_node");
    ros::NodeHandle n;
    ros::Subscriber ioSub = n.subscribe("car_data",10,LightSensor_cb);
    motorCtrlClient = n.serviceClient<rei_robot_base::Int8>("/set_extra_motor");
    ros::ServiceServer conveyerServer = n.advertiseService("conveyer_control",ConveyerCallback);
    ros::service::waitForService("/set_extra_motor");
    ros::MultiThreadedSpinner s(2);
    ros::spin(s);
    rei_robot_base::Int8 stop;
    stop.request.data = 0;
    motorCtrlClient.call(stop);
    return 0;
}